#include "arm_control/arm_action_type1.hpp"
#include <chrono>
#include <thread>

//A区抓取准备动作

namespace arm_control
{

ArmActionType1::ArmActionType1(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 初始化MoveGroupInterface，分别为左右机械臂
  move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_left");
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  // 初始化爪子发布器
  zhuazi_left_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_left", 10);
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
  
  RCLCPP_INFO(logger_, "MoveIt接口已初始化，Planning Group: arm_left 和 arm_right");
  RCLCPP_INFO(logger_, "arm_left规划参考坐标系: %s", move_group_left_->getPlanningFrame().c_str());
  RCLCPP_INFO(logger_, "arm_right规划参考坐标系: %s", move_group_right_->getPlanningFrame().c_str());
  RCLCPP_INFO(logger_, "创建爪子发布器: /zhuazi_left 和 /zhuazi_right");
}

void ArmActionType1::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型1 - 机械臂移动到A_left_pose和A_right_pose位置");
  
  // 发送初始反馈
  feedback->progress_percentage = 10.0;
  feedback->current_status = "设置目标姿态...";
  goal_handle->publish_feedback(feedback);
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型1已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型1已取消");
    return;
  }
  
  // 设置左臂命名目标姿态
  RCLCPP_INFO(logger_, "设置arm_left目标姿态为A_left_pose");
  move_group_left_->setNamedTarget("A_left_pose");
  
  // 设置右臂命名目标姿态（仅记录目标，不立即规划）
  RCLCPP_INFO(logger_, "设置arm_right目标姿态为A_right_pose");
  move_group_right_->setNamedTarget("A_right_pose");
  
  // 更新左臂起始状态为当前状态
  RCLCPP_INFO(logger_, "更新左臂起始状态为当前状态...");
  move_group_left_->setStartStateToCurrentState();
  
  feedback->progress_percentage = 25.0;
  feedback->current_status = "正在规划左臂路径...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "正在规划左臂路径...");
  
  // 规划左臂路径
  moveit::planning_interface::MoveGroupInterface::Plan left_plan;
  bool left_success = (move_group_left_->plan(left_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!left_success) {
    result->success = false;
    result->message = "动作类型1失败 - 左臂路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂路径规划成功！");
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型1已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型1已取消");
    return;
  }
  
  feedback->progress_percentage = 60.0;
  feedback->current_status = "正在执行左臂运动...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "开始执行左臂运动...");
  
  // 执行左臂规划好的路径
  auto left_execute_result = move_group_left_->execute(left_plan);
  
  if (left_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型1失败 - 左臂运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂运动执行成功！");
  
  // 发布爪子张开命令
  std_msgs::msg::Int32 left_msg;
  left_msg.data = 120;
  zhuazi_left_publisher_->publish(left_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_left: 120 (张开)");
  
  // 等待1.5秒让爪子动作完成
  RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
  std::this_thread::sleep_for(std::chrono::milliseconds(1500));
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型1已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型1已取消");
    return;
  }
  
  // 左臂执行完成后，等待1秒
  feedback->progress_percentage = 65.0;
  feedback->current_status = "等待1秒让机械臂稳定...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "左臂执行完成，等待1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  // 更新右臂起始状态为当前状态（包括左臂执行后改变的共享关节xuan1）
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态...");
  move_group_right_->setStartStateToCurrentState();
  
  // 开始规划右臂路径
  RCLCPP_INFO(logger_, "开始规划右臂路径...");
  
  // 规划右臂路径
  feedback->progress_percentage = 70.0;
  feedback->current_status = "正在规划右臂路径...";
  goal_handle->publish_feedback(feedback);
  
  moveit::planning_interface::MoveGroupInterface::Plan right_plan;
  bool right_success = (move_group_right_->plan(right_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!right_success) {
    result->success = false;
    result->message = "动作类型1失败 - 右臂规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂规划成功");
  
  feedback->progress_percentage = 80.0;
  feedback->current_status = "正在执行右臂运动...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "开始执行右臂运动...");
  
  // 执行右臂规划的路径
  auto right_execute_result = move_group_right_->execute(right_plan);
  
  if (right_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型1失败 - 右臂运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂运动执行成功！");
  
  // 发布爪子张开命令
  std_msgs::msg::Int32 right_msg;
  right_msg.data = 120;
  zhuazi_right_publisher_->publish(right_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120 (张开)");
  
  // 等待1.5秒让爪子动作完成
  RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
  std::this_thread::sleep_for(std::chrono::milliseconds(1500));
  
  // 最终反馈
  feedback->progress_percentage = 100.0;
  feedback->current_status = "左右臂已到达目标位置";
  goal_handle->publish_feedback(feedback);
  
  // 动作完成
  result->success = true;
  result->message = "动作类型1执行成功 - arm_left已移动到A_left_pose，arm_right已移动到A_right_pose";
  goal_handle->succeed(result);
  RCLCPP_INFO(logger_, "动作类型1执行成功 - 左右臂已到达目标位置");
}

} // namespace arm_control
